#ifndef RSIM_BODY3D_H
#define RSIM_BODY3D_H

/* -------------------------------------------------------------------------- *
* File: Body3D.h                                                             *
* Authors: Kishor Bhalerao                                                   *
* Email : kishor8dm@gmail.com                                                *
* Contributors:                                                              *
* Email:                                                                     *
* copyright (c) 2010 Authors.                                                *
*                                                                            *
* This program is free software: you can redistribute it and/or modify it    *
* under the terms of the GNU General Public License as published by the Free *
* Software Foundation, either version 3 of the License, or any later version.*
*                                                                            *
* This program is distributed in the hope that it will be useful, but        *
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
* or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
* for more details. (http://www.gnu.org/licenses/)                           *
* -------------------------------------------------------------------------- */

#include "EOMData3D.h"
#include "ABAData3D.h"
#include "DCAData3D.h"

namespace RSIM{

const static double& lambda_null = 10000000.0;
const static double k[] = {lambda_null,0,0,0,lambda_null,0,0,0,lambda_null};
const static Mat3x3 Earth(k);
        
struct LillyData{
        
        Mat6x6 Lambda;
        Vect6 p;
        
        Vect6 fk;                
        Mat6x6 L;
        double xk;
};
        
/** Rigid body */
class Body3D{
        private:                
                Body3D(const Body3D&);
        public:
                Body3D(){}
                
                void init();
                
                double len_;
                double mass_;                
                Mat3x3 inertia_;                                                                                                                        
                Mat3x3 Jb_;
                Mat3x3 n_C_k_;
                Mat3x3 R_FM_;
                Mat3x3 R_NM_;
                
                Mat3x3 itilde;
                double itil;
                Mat6x6 Iskbeta_;
                                
                EOMData3D eom_;                
                ABAData3D aba_;
                DCAData3D dca_;
                
                LillyData Dat_;
                
                int id_;
                
                double q_;
                double u_;
                double udot_;
                double tau_;
                
                // Udot multipliers                
                Vect6 m1_;
                double m2_;
                Vect6 m3_;
                
                Vect3 v_in_;
                Vect3 v_on_;
                Vect6 jac_;                
                
                Mat6x6 psi1_;
                Vect6 psi2_;
                Mat6x6 psi3_;
                
                Mat6x6 PSI1_;
                Vect6 PSI2_;
                Mat6x6 PSI3_;
                
                Vect3 w_;
                Vect3 v_;
                Vect6 Akt_;
                double rk_,betak_;
                Vect3 nrk_;
                Vect3 nbetak_;
                Vect3 c_nbetak_;
                Mat6x6 nzeta12_; // 6x5
                
                ////////////////////////////
                // functions. 
                
                void calc_xk(const double *rot,
                             const double *lambda,
                             double &xk);
                
                void calc_L(    const double *rot,
                                const double *lambda,
                                const double& xk,
                                double *L);
                                
                void calc_Lfk_pFk(const double *,
                                  const double *,
                                  const double *,
                                  double *);
                                
                void calcLambdaWithParentLilly( const double *,
                                                const double *,
                                                const double *,
                                                const double *,
                                                const double *,
                                                const double &,
                                                double *);
                
                void calc_itil(const double *rot,
                               const double *I, 
                               double& itil);
                void calcJ();
                void calcJ(const Body3D *child);
                void calcJFromvin();
                void calcJTx(const double *x);
                void calcJTx(const Body3D *child,
                             const double *x);
                               
                void updRotationMatrix(const bool& updVec=true);
                void updRotationMatrix(const Mat3x3& R_NK);
                void updRotationMatrix(const Body3D *parent,const bool& updVec=true);                                
                void updcbetak(const Body3D *parent);
                void updcbetak(const Mat3x3& Rot);
                void updvon(const Body3D& parent);
                void updvon();
                void calcvin(const double *von);
                void calcNullSpaceProjectionMatrixRow(const int& nbodies,
                                                   const double *pinvjac,
                                                   double *null);
                
                void updAngularVelocity();
                void updAngularVelocity(const Body3D& Root);
                void updAngularAndLinearVelocity(const Body3D& Root,const Body3D *parent, double *t);                                        
                void updAngularVelocity(const Body3D *parent,const bool& calcAkt=true);
                
                void calcLambdaNoParentLilly(const double *nrk,
                                        const double *Jk1,
                                        const double &lnull,
                                        double *M);
                
                void n_formInboardEOM(const Vect3& gravity);
                void n_formInboardEOM(const Body3D *parent, const Vect3& gravity);
                void n_triangularizeBackward(   const Body3D *parent,
                                                const Body3D *child);
                void n_substituteForward(const Body3D* parent);
                
                void n_calcZeta12Backward();
                
                void n_calcZeta12Backward(const Body3D *child);
                
                void n_calcZetaForward_Opt(const int& rootP1,
                                           const Body3D *parent,
                                           const bool& calcH2=true);
                                           
                void n_shiftPhiToOutboardHandle_Opt(const Body3D *Root,
                                                    const bool& nbody=true);
                
                void n_formDCAHandleEquations_Opt(const Vector& gravity);
                
                void n_sweepForwardLillyOrin(const Body3D *Parent);
                
                ////////////////////////////
                // Utilities
                
                void setIandF(double *I,
                              double *F);
                                      
                void Wx_J_W(const double *w, 
                            const double *c, 
                            double *p);
                            
                void calc_cFk(const double *Fk3, 
                              const double *Ik3, 
                              const double *Akt, 
                              double *cFk) const;
                
                void calc_cFk_01(const double *Fk3, 
                                const double *Ik3, 
                                const double *Akt, 
                                double *cFk) const;
                                
                void calc_m_cFk(const double *F, 
                                const double *I, 
                                const double *A, 
                                double *mF) const;
                            
                void calcTriang(const double *beta,
                                const double *I,
                                const double *Rot,
                                const double& itil,
                                double *Tr);
                                
                void calcTriang_I_tBeta(const double *Tr,
                                        const double *I,
                                        const double *beta,
                                        double *ik3c);
                                
                void calc_phiOneAndTwo( const double *beta,
                                        const double *p_phi,
                                        const double *m,
                                        double *phi);
                void calc_I_x_transposeskbeta(const double *I, const double *beta, double *I_X_beta);
};
} // namespace RSIM
#endif